/*
 * Droplet.cpp
 *
 * Created: 5/11/2012 4:13:57 PM
 *  Author: common
 */ 
#define F_CPU 32000000UL

#include <avr/io.h>
#include <math.h>
#include <util/delay.h>

#include "motor.h"
#include "RGB_LED.h"
#include "pc_com.h"
#include "RGB_sensor.h"
#include "power.h"
#include "random.h"

extern uint8_t rsenbase,gsenbase,bsenbase;
char userPause = ' ';

void Config32MHzClock(void)
{
	// initialize clock source to be 32MHz internal oscillator (no PLL)
	CCP = CCP_IOREG_gc;							//Security Signature to modify clock
	OSC.CTRL = OSC_RC32MEN_bm;					// enable internal 32MHz oscillator
	while(!(OSC.STATUS & OSC_RC32MRDY_bm));		// wait for oscillator ready
	CCP = CCP_IOREG_gc;							//Security Signature to modify clock
	CLK.CTRL = 0x01;							//select sysclock 32MHz osc
}

uint8_t findPower()
{
	uint8_t duty_cycle = 192, onTime = 200, offTime = 200; 
	
	uint8_t mot = 1, hadPower = 1;
	uint8_t oldtcc0cca = TCC0_CCA;
	uint8_t oldtcc0ccb = TCC0_CCB;
	uint8_t oldtcc1cca = TCC1_CCA;
	uint8_t oldtcc1ccb = TCC1_CCB;
	uint8_t oldtce0cca = TCE0_CCA;
	uint8_t oldtce0ccb = TCE0_CCB;
	
	TCC0_CCA = TCC0_CCB = duty_cycle;
	TCC1_CCA = TCC1_CCB = duty_cycle;
	TCE0_CCA = TCE0_CCB = duty_cycle;

	while (!((leg1_status()==1 || leg2_status()==1 || leg3_status()==1) && (leg1_status()==-1 || leg2_status()==-1 || leg3_status()==-1)))
	{
		_delay_ms(200);
		if ((leg1_status()==1 || leg2_status()==1 || leg3_status()==1) && (leg1_status()==-1 || leg2_status()==-1 || leg3_status()==-1))
			break;
	
		printf("LEG 1 IS %i, LEG 2 IS %i, LEG 3 IS %i\r\n", leg1_status(), leg2_status(), leg3_status());
		hadPower = 0;
		
		for (int8_t i = 0; i < 3; i++)
		{
			if (cap_status() == 0) set_blue_led(50);
			else set_red_led(50);
			_delay_ms(30);
			led_off();
			_delay_ms(70);
		}

	    switch (mot)
		{
			case (1) : TCC1.CTRLB = TC1_CCAEN_bm | TC_WGMODE_SS_gc; _delay_ms(onTime); TCC1.CTRLB = TC_WGMODE_SS_gc; break;
			case (2) : TCC0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SS_gc; _delay_ms(onTime); TCC0.CTRLB = TC_WGMODE_SS_gc; break;
			case (3) : TCE0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SS_gc; _delay_ms(onTime); TCE0.CTRLB = TC_WGMODE_SS_gc; break;
			case (4) : TCC1.CTRLB = TC1_CCBEN_bm | TC_WGMODE_SS_gc; _delay_ms(onTime); TCC1.CTRLB = TC_WGMODE_SS_gc; break;
			case (5) : TCC0.CTRLB = TC0_CCBEN_bm | TC_WGMODE_SS_gc; _delay_ms(onTime); TCC0.CTRLB = TC_WGMODE_SS_gc; break;
			case (6) : TCE0.CTRLB = TC0_CCBEN_bm | TC_WGMODE_SS_gc; _delay_ms(onTime); TCE0.CTRLB = TC_WGMODE_SS_gc; break;
		}
		_delay_ms(offTime);
		
		mot++;
		if (mot > 6) mot = 1;
	}
	
	if (!hadPower)
	{
		led_off(); _delay_ms(20); set_green_led(50); _delay_ms(80);
		led_off(); _delay_ms(20); set_green_led(50); _delay_ms(80);
		led_off(); _delay_ms(20); set_green_led(50); _delay_ms(80);
		led_off();		
	}
	
	_delay_ms(1000);
	
	TCC0_CCA = oldtcc0cca;
	TCC0_CCB = oldtcc0ccb;
	TCC1_CCA = oldtcc1cca;
	TCC1_CCB = oldtcc1ccb;
	TCE0_CCA = oldtce0cca;
	TCE0_CCB = oldtce0ccb;

	return hadPower;
}

char doUserPause()
{
	findPower();
	if (PC_USART.STATUS & USART_RXCIF_bm)
	{
		userPause = PC_USART.DATA;
		printf("\r\nPaused (%c) \r\n", userPause);
		if (userPause == 'p')
		{
			while (!(PC_USART.STATUS & USART_RXCIF_bm))
			{
				set_green_led(50); set_red_led(50);
				_delay_ms(50); led_off(); _delay_ms(250);
			}
			userPause = PC_USART.DATA;
		}			
		return userPause;
	}
	
	if (get_red_sensor() > 15)
	{
		printf("\r\nPaused (Red light, %i)\r\n", get_red_sensor());
		for (uint8_t i = 0; i < 10; i++)
		{
			set_red_led(50); set_blue_led(50);
			_delay_ms(50); led_off(); _delay_ms(250);
		}
		rgb_sensor_init();
		userPause = 'R';
		return userPause;
	}

	if (get_green_sensor() > 10)
	{
		printf("\r\nPaused (Green light, %i)\r\n", get_green_sensor());
		for (uint8_t i = 0; i < 10; i++)
		{
			set_green_led(50); set_blue_led(50);
			_delay_ms(50); led_off(); _delay_ms(250);
		}
		rgb_sensor_init();
		userPause = 'G';
		return userPause;
	}
	
	return userPause;
}

void colorCycle()
{
	doUserPause();
	stop();
	led_off();
	for (uint8_t r = 0; r < 255; r++)
	{
		set_red_led(r);
		_delay_ms(2);
	}
	for (uint8_t r = 255; r > 0; r--)
	{
		set_red_led(r);
		_delay_ms(2);
	}
	set_red_led(0);
	doUserPause();
			
	for (uint8_t g = 0; g < 255; g++)
	{
		set_green_led(g);
		_delay_ms(2);
	}
	for (uint8_t g = 255; g > 0; g--)
	{
		set_green_led(g);
		_delay_ms(2);
	}
	set_green_led(0);
	doUserPause();
			
	for (uint8_t b = 0; b < 255; b++)
	{
		set_blue_led(b);
		_delay_ms(2);
	}
	for (uint8_t b = 255; b > 0; b--)
	{
		set_blue_led(b);
		_delay_ms(2);
	}
	set_blue_led(0);
	doUserPause();
}

void motorCycle()
{
	uint8_t duty_cycle = 150;
	TCC0_CCA = TCC0_CCB = duty_cycle;
	TCC1_CCA = TCC1_CCB = duty_cycle;
	TCE0_CCA = TCE0_CCB = duty_cycle;

	doUserPause();
	for (uint8_t i = 0; i < 2; i++)
	{
		set_red_led(75); _delay_ms(500); doUserPause(); led_off(); _delay_ms(500); doUserPause();
	}
	set_green_led(75); _delay_ms(1000); doUserPause(); led_off(); _delay_ms(100);

	TCC1.CTRLB = TC1_CCAEN_bm | TC_WGMODE_SS_gc; _delay_ms(500); TCC1.CTRLB = TC_WGMODE_SS_gc; _delay_ms(500); doUserPause();
	TCC0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SS_gc; _delay_ms(500); TCC0.CTRLB = TC_WGMODE_SS_gc; _delay_ms(500); doUserPause();
	TCE0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SS_gc; _delay_ms(500); TCE0.CTRLB = TC_WGMODE_SS_gc; _delay_ms(500); doUserPause();

	set_red_led(40); _delay_ms(100); led_off(); _delay_ms(400);
	
	TCC1.CTRLB = TC1_CCBEN_bm | TC_WGMODE_SS_gc; _delay_ms(500); TCC1.CTRLB = TC_WGMODE_SS_gc; _delay_ms(500); doUserPause();
	TCC0.CTRLB = TC0_CCBEN_bm | TC_WGMODE_SS_gc; _delay_ms(500); TCC0.CTRLB = TC_WGMODE_SS_gc; _delay_ms(500); doUserPause();
	TCE0.CTRLB = TC0_CCBEN_bm | TC_WGMODE_SS_gc; _delay_ms(500); TCE0.CTRLB = TC_WGMODE_SS_gc; _delay_ms(500); doUserPause();

	stop();
}

void colorWalk()
{
	uint8_t r=0, g=0, b=0;
	set_rgb(r, g, b);
	
	doUserPause();
	for (uint16_t i = 0; i < 1000; i++)
	{
		uint8_t ran = rand_byte();
		r += (ran & 0x03) - 1;
		r &= 0x7F;
		g += ((ran & 0x0B) >> 2) - 1;
		b += ((ran & 0x30) >> 4) - 1;
		printf("(%i,%i,%i)",r,g,b);
		set_rgb(r,g,b);
		_delay_ms(5);
	}
	led_off();
}

void colorSine()
{
	float rs=0.001f, gs = 0.001f, bs = 0.001f;
	float rt=0, gt=2.1, bt=4.2;
	uint8_t r=0, g=0, b=0;
	led_off();
	doUserPause();

	for  (uint16_t i = 0; i < 20000; i++)
	{
		r = (uint8_t)(127.0f * (float)sin(rt) + 127.0f);
		g = (uint8_t)(127.0f * (float)sin(gt) + 127.0f);
		b = (uint8_t)(127.0f * (float)sin(bt) + 127.0f);
		rt += rs; gt += gs; bt += bs;
		printf("(%i,%i,%i)",r,g,b);
		set_rgb(r,g,b);
		_delay_ms(0.25);
	}	

	led_off();	
	doUserPause();
}

int main(void)
{
	Config32MHzClock();
	
	motor_init();
	RGB_LED_init();
	rgb_sensor_init();
	leg_monitor_init();
	cap_monitor_init();
	pc_com_init();
	random_init();
	
	set_blue_led(50); _delay_ms(250); led_off();
	_delay_ms(250); set_blue_led(50); _delay_ms(250); led_off();
	
	int16_t loopnum = 0;
	while (1)
	{
		doUserPause();
		
		printf("Iteration %i \r\n", ++loopnum);
		
		printf("\tColorCycle\r\n");
		colorCycle();
		
		_delay_ms(500);
		
		printf("\tColorSine\r\n");
		colorSine();
		
		_delay_ms(500);
		
		if (userPause != 'R')
		{
			printf("\tMotorCycle\r\n");
			motorCycle();
			stop();
		
			doUserPause();
			_delay_ms(500);
		}			
	}	
}